const express = require("express");
const path = require("path");
const net = require("net");
const WebSocket = require("ws");
const fs = require('fs');

// Draco解码器
let dracoDecoderModule = null;
let decoderInitialized = false;

const app = express();
const HTTP_PORT = 3000;
const WS_PORT = 8080;
const TCP_PORT = 9998;

// 1. Express托管静态网页
app.use(express.static(path.join(__dirname, "public")));

app.listen(HTTP_PORT, () => {
  console.log(`HTTP server running at http://localhost:${HTTP_PORT}`);
});

// 2. WebSocket服务器，用来推送点云数据给网页
const wss = new WebSocket.Server({ port: WS_PORT });
let clients = [];

wss.on("connection", (ws) => {
  clients.push(ws);
  console.log("WebSocket client connected");

  ws.on("close", () => {
    clients = clients.filter(c => c !== ws);
    console.log("WebSocket client disconnected");
  });
});


function broadcastPointCloud(buffer) {
  console.log(`Broadcasting ${buffer.byteLength} bytes to ${clients.length} clients`);
  for (const ws of clients) {
    if (ws.readyState === WebSocket.OPEN) {
      ws.send(buffer);
    }
  }
}
// 初始化Draco解码器
// 修复的Draco初始化
async function initDracoDecoder() {
  console.log("Initializing Draco decoder...");
  
  // 方法1: 尝试不同的draco3d导入方式
  try {
    const draco = require('draco3d');
    console.log("draco3d module loaded, type:", typeof draco);
    
    // 检查draco的结构
    if (typeof draco === 'function') {
      dracoDecoderModule = await draco();
    } else if (draco.createDecoderModule) {
      dracoDecoderModule = await draco.createDecoderModule();
    } else if (draco.DracoDecoderModule) {
      dracoDecoderModule = await draco.DracoDecoderModule();
    } else if (draco.default) {
      dracoDecoderModule = await draco.default();
    } else {
      dracoDecoderModule = draco;
    }
    
    decoderInitialized = true;
    console.log("Draco decoder initialized with draco3d");
    console.log("Available methods:", Object.keys(dracoDecoderModule).slice(0, 10));
    return true;
    
  } catch (error) {
    console.error("draco3d failed:", error.message);
  }

  // 方法2: 尝试draco3dgltf
  try {
    const dracoGltf = require('draco3dgltf');
    console.log("draco3dgltf module loaded, type:", typeof dracoGltf);
    
    if (dracoGltf.createDecoderModule) {
      dracoDecoderModule = await dracoGltf.createDecoderModule();
    } else if (typeof dracoGltf === 'function') {
      dracoDecoderModule = await dracoGltf();
    } else {
      dracoDecoderModule = dracoGltf;
    }
    
    decoderInitialized = true;
    console.log("Draco decoder initialized with draco3dgltf");
    return true;
    
  } catch (error) {
    console.error("draco3dgltf failed:", error.message);
  }

  // 方法3: 手动加载WASM
  try {
    const path = require('path');
    
    // 检查可能的draco库位置
    const possiblePaths = [
      'draco3d',
      'gltf-pipeline/lib/draco',
      '@google-cloud/draco',
    ];
    
    for (const libPath of possiblePaths) {
      try {
        const dracoLib = require(libPath);
        console.log(`Found draco at ${libPath}:`, typeof dracoLib);
        
        if (typeof dracoLib === 'function') {
          dracoDecoderModule = await dracoLib();
        } else if (dracoLib.createDecoderModule) {
          dracoDecoderModule = await dracoLib.createDecoderModule();
        } else {
          dracoDecoderModule = dracoLib;
        }
        
        decoderInitialized = true;
        console.log(`Draco decoder initialized with ${libPath}`);
        return true;
      } catch (e) {
        console.log(`${libPath} not available`);
      }
    }
  } catch (error) {
    console.error("Manual WASM loading failed:", error.message);
  }

  console.error("All Draco initialization methods failed");
  return false;
}
// 简化的Draco解压缩函数
function decompressDracoPointCloud(compressedData) {
  if (!decoderInitialized || !dracoDecoderModule) {
    throw new Error("Draco decoder not initialized");
  }

  console.log(`Starting Draco decompression of ${compressedData.length} bytes`);

  const decoder = new dracoDecoderModule.Decoder();
  const buffer = new dracoDecoderModule.DecoderBuffer();
  let pointCloud = null;
  
  try {
    // 将Buffer转换为Uint8Array
    const uint8Array = new Uint8Array(compressedData);
    buffer.Init(uint8Array, uint8Array.length);
    
    // 检查几何类型
    const geometryType = decoder.GetEncodedGeometryType(buffer);
    console.log(`Geometry type: ${geometryType}, Expected POINT_CLOUD: ${dracoDecoderModule.POINT_CLOUD}`);
    
    if (geometryType !== dracoDecoderModule.POINT_CLOUD) {
      throw new Error(`Invalid geometry type: ${geometryType}, expected POINT_CLOUD`);
    }
    
    // 解码点云
    pointCloud = new dracoDecoderModule.PointCloud();
    const status = decoder.DecodeBufferToPointCloud(buffer, pointCloud);
    
    if (!status.ok()) {
      throw new Error(`Decode failed with status: ${status.code()}`);
    }
    
    const numPoints = pointCloud.num_points();
    console.log(`Successfully decoded ${numPoints} points`);
    
    // 获取所有属性
    const numAttributes = pointCloud.num_attributes();
    console.log(`Total attributes: ${numAttributes}`);
    
    let positionAttribute = null;
    let intensityAttribute = null;
    
    for (let i = 0; i < numAttributes; i++) {
      const attr = decoder.GetAttribute(pointCloud, i);
      const attrType = attr.attribute_type();
      const numComponents = attr.num_components();
      
      console.log(`Attribute ${i}: type=${attrType}, components=${numComponents}, size=${attr.size()}`);
      
      if (attrType === dracoDecoderModule.POSITION) {
        positionAttribute = attr;
        console.log("Found position attribute at index", i);
      } else if (attrType === dracoDecoderModule.GENERIC) {
        intensityAttribute = attr;
        console.log("Found intensity attribute at index", i);
      }
    }
    
    if (!positionAttribute) {
      throw new Error("No position attribute found");
    }
    
    // 创建输出缓冲区
    const outputData = new Float32Array(numPoints * 4);
    
    // 使用正确的批量提取方法
    console.log("Extracting position data...");
    
    // 提取位置数据
    const positionArray = new dracoDecoderModule.DracoFloat32Array();
    const positionSuccess = decoder.GetAttributeFloatForAllPoints(
      pointCloud, positionAttribute, positionArray);
    
    if (!positionSuccess) {
      throw new Error("Failed to extract position data");
    }
    
    console.log("Position data extraction successful, array size:", positionArray.size());
    
    // 提取强度数据
    let intensityArray = null;
    if (intensityAttribute) {
      intensityArray = new dracoDecoderModule.DracoFloat32Array();
      const intensitySuccess = decoder.GetAttributeFloatForAllPoints(
        pointCloud, intensityAttribute, intensityArray);
      
      if (intensitySuccess) {
        console.log("Intensity data extraction successful, array size:", intensityArray.size());
      } else {
        console.warn("Failed to extract intensity data");
        dracoDecoderModule.destroy(intensityArray);
        intensityArray = null;
      }
    }
    
    // 组装输出数据
    console.log("Assembling output data...");
    for (let i = 0; i < numPoints; i++) {
      const outputIndex = i * 4;
      const posIndex = i * 3;
      
      // 复制位置数据 (x, y, z)
      outputData[outputIndex] = positionArray.GetValue(posIndex);         // x
      outputData[outputIndex + 1] = positionArray.GetValue(posIndex + 1); // y
      outputData[outputIndex + 2] = positionArray.GetValue(posIndex + 2); // z
      
      // 复制强度数据
      if (intensityArray) {
        outputData[outputIndex + 3] = intensityArray.GetValue(i); // intensity
      } else {
        outputData[outputIndex + 3] = 0.0; // 默认强度
      }
    }
    
    // 打印前几个点进行验证
    // console.log("First few decoded points:");
    // for (let i = 0; i < Math.min(3, numPoints); i++) {
    //   const idx = i * 4;
    //   console.log(`Point ${i}: x=${outputData[idx]}, y=${outputData[idx+1]}, z=${outputData[idx+2]}, intensity=${outputData[idx+3]}`);
    // }
    
    // 检查数据有效性
    let nonZeroCount = 0;
    for (let i = 0; i < Math.min(100, numPoints); i++) {
      const idx = i * 4;
      if (outputData[idx] !== 0 || outputData[idx+1] !== 0 || outputData[idx+2] !== 0) {
        nonZeroCount++;
      }
    }
    console.log(`Non-zero points in first 100: ${nonZeroCount}`);
    
    // 清理Draco资源
    dracoDecoderModule.destroy(positionArray);
    if (intensityArray) {
      dracoDecoderModule.destroy(intensityArray);
    }
    dracoDecoderModule.destroy(pointCloud);
    dracoDecoderModule.destroy(buffer);
    dracoDecoderModule.destroy(decoder);
    
    return outputData.buffer;
    
  } catch (error) {
    // 确保清理资源
    console.error("Decompression error:", error);
    try {
      if (pointCloud) dracoDecoderModule.destroy(pointCloud);
      if (buffer) dracoDecoderModule.destroy(buffer);
      if (decoder) dracoDecoderModule.destroy(decoder);
    } catch (cleanupError) {
      console.error("Error during cleanup:", cleanupError);
    }
    throw error;
  }
}



// 备用：不使用Draco，直接传输原始数据
function processAsRawData(data) {
  console.log("Processing as raw float data...");
  // 假设数据是连续的float数组 (x, y, z, intensity)
  return data.buffer;
}



// 3. TCP服务，监听ROS节点发送的压缩点云数据
// TCP服务器
const tcpServer = net.createServer((socket) => {
  console.log("TCP client connected:", socket.remoteAddress);

  let dataBuffer = Buffer.alloc(0);

  socket.on("data", (chunk) => {
    dataBuffer = Buffer.concat([dataBuffer, chunk]);
    
    // 至少需要4字节头部
    while (dataBuffer.length >= 4) {
      // 读取数据大小（小端序）
      const expectedSize = dataBuffer.readUInt32LE(0);
      
      console.log(`Expected data size: ${expectedSize} bytes, current buffer: ${dataBuffer.length} bytes`);
      
      // 检查数据大小是否合理
      if (expectedSize > 50 * 1024 * 1024) { // 50MB限制
        console.error(`Data size too large: ${expectedSize} bytes, resetting connection`);
        socket.destroy();
        return;
      }
      
      // 检查是否接收完整个包
      if (dataBuffer.length >= 4 + expectedSize) {
        const actualData = dataBuffer.slice(4, 4 + expectedSize);
        
        console.log(`Processing complete packet: ${actualData.length} bytes`);
        
        // 处理数据
        try {
          let resultBuffer;
          
          if (decoderInitialized) {
            // 尝试Draco解压
            resultBuffer = decompressDracoPointCloud(actualData);
          } else {
            // 备用方案：当作原始数据处理
            console.log("Draco not available, treating as raw data");
            resultBuffer = processAsRawData(actualData);
          }
          
          broadcastPointCloud(resultBuffer);
          
        } catch (error) {
          console.error("Data processing error:", error);
          
          // 保存错误数据用于调试
          fs.writeFileSync(`debug_data_${Date.now()}.bin`, actualData);
          console.log("Saved problematic data for debugging");
        }
        
        // 移除已处理的数据
        dataBuffer = dataBuffer.slice(4 + expectedSize);
      } else {
        // 数据不完整，等待更多数据
        break;
      }
    }
  });

  socket.on("end", () => {
    console.log("TCP client disconnected");
  });

  socket.on("error", (err) => {
    console.error("TCP socket error:", err);
  });
});
// 启动服务器
async function startServer() {
  console.log("Starting server...");
  
  // 尝试初始化Draco
  const dracoSuccess = await initDracoDecoder();
  if (!dracoSuccess) {
    console.log("Warning: Draco decoder not available. Will process data as raw format.");
  }
  
  // 启动TCP服务器
  tcpServer.listen(TCP_PORT, () => {
    console.log("TCP server listening on port 8080");
    console.log(`Draco decoder status: ${decoderInitialized ? 'Ready' : 'Not available'}`);
  });
}

// 启动
startServer().catch(console.error);